﻿#include "multi_agent_solver.h"

// Ensure iostream is available for ECBS's debug printing (std::cout/endl)
#include "kernel/interfaces/ILogger.h"
#include <QHash>
#include <QString>
#include <QStringList>
#include <boost/functional/hash.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <cassert>
#include <cmath>
#include <iostream>
#include <functional>
#include <exception>
#include <libMultiRobotPlanning/ecbs.hpp>
#include <libMultiRobotPlanning/planresult.hpp>
#include <map>
#include <queue>
#include <tuple>
#include <utility>
#include <unordered_map>
#include <unordered_set>

namespace application {
namespace pathfinding {

// roadmap data structures (from solve.h)
using roadmapTraits_t =
    boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS>;
using vertex_t = roadmapTraits_t::vertex_descriptor;
using edge_t = roadmapTraits_t::edge_descriptor;

// Custom hasher for edge descriptors
struct EdgeHash {
  size_t operator()(const edge_t& e) const noexcept {
    size_t seed = 0;
    boost::hash_combine(seed, e.m_source);
    boost::hash_combine(seed, e.m_target);
    return seed;
  }
};

struct Vertex {
  std::string name;
  bool avoidWait{false};
  std::string blockAreaId;
};

struct Edge {
  std::unordered_set<edge_t, EdgeHash> conflictingEdges;
  int cost{1};
};

struct CanceledSearch : public std::exception {
  const char* what() const noexcept override { return "MAPF search canceled"; }
};

using roadmap_t = boost::adjacency_list<boost::vecS, boost::vecS,
                                        boost::directedS, Vertex, Edge>;

// State (from solve.h)
struct State {
  State() = default;
  State(int time, vertex_t vertex) : time(time), vertex(vertex) {}

  bool operator==(const State& s) const {
    return time == s.time && vertex == s.vertex;
  }

  bool equalExceptTime(const State& s) const { return vertex == s.vertex; }

  int time{0};
  vertex_t vertex{};
};

// Stream operator for debugging output required by ECBS
inline std::ostream& operator<<(std::ostream& os, const State& s) {
  return os << s.time << ": (" << s.vertex << ")";
}

using Action = edge_t;

// Provide std::hash specialization for State before template instantiation
}  // namespace pathfinding
}  // namespace application

namespace std {
template <>
struct hash<application::pathfinding::State> {
  size_t operator()(const application::pathfinding::State& s) const noexcept {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.vertex);
    return seed;
  }
};
}  // namespace std

namespace application {
namespace pathfinding {

// Conflict (from solve.cpp)
struct Conflict {
  enum Type {
    Vertex,
    Edge,
    BlockArea,
  };

  int time;
  size_t agent1;
  size_t agent2;
  Type type;

  vertex_t vertex;
  edge_t edge1;
  edge_t edge2;
  std::string blockAreaId;
};

// VertexConstraint (from solve.cpp)
struct VertexConstraint {
  VertexConstraint(int time, vertex_t vertex) : time(time), vertex(vertex) {}
  int time;
  vertex_t vertex;

  bool operator<(const VertexConstraint& other) const {
    return std::tie(time, vertex) < std::tie(other.time, other.vertex);
  }

  bool operator==(const VertexConstraint& other) const {
    return std::tie(time, vertex) == std::tie(other.time, other.vertex);
  }
};

// EdgeConstraint (from solve.cpp)
struct EdgeConstraint {
  EdgeConstraint(int time, edge_t edge) : time(time), edge(edge) {}
  int time;
  edge_t edge;

  bool operator<(const EdgeConstraint& other) const {
    return std::tie(time, edge) < std::tie(other.time, other.edge);
  }

  bool operator==(const EdgeConstraint& other) const {
    return std::tie(time, edge) == std::tie(other.time, other.edge);
  }
};

// Constraints (from solve.cpp)
// Custom hashers to avoid late specializations
struct VertexConstraintHash {
  size_t operator()(const VertexConstraint& s) const noexcept {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.vertex);
    return seed;
  }
};
struct EdgeConstraintHash {
  size_t operator()(const EdgeConstraint& s) const noexcept {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.edge);
    return seed;
  }
};

struct Constraints {
  std::unordered_set<VertexConstraint, VertexConstraintHash> vertexConstraints;
  std::unordered_set<EdgeConstraint, EdgeConstraintHash> edgeConstraints;

  void add(const Constraints& other) {
    vertexConstraints.insert(other.vertexConstraints.begin(),
                             other.vertexConstraints.end());
    edgeConstraints.insert(other.edgeConstraints.begin(),
                           other.edgeConstraints.end());
  }

  bool overlap(const Constraints& other) const {
    for (const auto& vc : vertexConstraints) {
      if (other.vertexConstraints.count(vc) > 0) {
        return true;
      }
    }
    for (const auto& ec : edgeConstraints) {
      if (other.edgeConstraints.count(ec) > 0) {
        return true;
      }
    }
    return false;
  }
};

// Environment class (from solve.cpp) - complete implementation
class Environment {
 public:
  Environment(const roadmap_t& roadmap, const std::vector<vertex_t>& goals,
              bool disappearAtGoal = false)
      : m_roadmap(roadmap),
        m_goals(std::move(goals)),
        m_agentIdx(0),
        m_constraints(nullptr),
        m_lastGoalConstraint(-1),
        m_highLevelExpanded(0),
        m_lowLevelExpanded(0),
        m_disappearAtGoal(disappearAtGoal) {
    const size_t vertexCount = boost::num_vertices(m_roadmap);
    m_vertexBlockArea.resize(vertexCount);
    for (size_t v = 0; v < vertexCount; ++v) {
      const auto& areaId = m_roadmap[static_cast<vertex_t>(v)].blockAreaId;
      m_vertexBlockArea[v] = areaId;
      if (!areaId.empty()) {
        m_blockAreaVertices[areaId].push_back(static_cast<vertex_t>(v));
      }
    }
  }

  Environment(const Environment&) = delete;
  Environment& operator=(const Environment&) = delete;

  void setLowLevelContext(size_t agentIdx, const Constraints* constraints) {
    checkCanceled();
    assert(constraints);
    m_agentIdx = agentIdx;
    m_constraints = constraints;
    m_lastGoalConstraint = -1;
    for (const auto& vc : constraints->vertexConstraints) {
      if (vc.vertex == m_goals[m_agentIdx]) {
        m_lastGoalConstraint = std::max(m_lastGoalConstraint, vc.time);
      }
    }
  }

  void setCancelCallback(std::function<bool()> cancelCb) {
    m_cancelCallback = std::move(cancelCb);
  }

  int admissibleHeuristic(const State& s) {
    checkCanceled();
    return 0;
  }

  // low-level
  int focalStateHeuristic(
      const State& s, int /*gScore*/,
      const std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>>&
          solution) {
    checkCanceled();
    int numConflicts = 0;
    for (size_t i = 0; i < solution.size(); ++i) {
      if (i != m_agentIdx && !solution[i].states.empty()) {
        State state2 = getState(i, solution, s.time);
        if (s.equalExceptTime(state2)) {
          ++numConflicts;
        }
      }
    }
    return numConflicts;
  }

  // low-level
  int focalTransitionHeuristic(
      const State& s1a, const State& s1b, int /*gScoreS1a*/, int /*gScoreS1b*/,
      const std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>>&
          solution) {
    checkCanceled();
    int numConflicts = 0;
    for (size_t i = 0; i < solution.size(); ++i) {
      if (i != m_agentIdx && !solution[i].states.empty()) {
        State s2a = getState(i, solution, s1a.time);
        State s2b = getState(i, solution, s1b.time);
        if (s1a.equalExceptTime(s2b) && s1b.equalExceptTime(s2a)) {
          ++numConflicts;
        }
      }
    }
    return numConflicts;
  }

  // Count all conflicts
  int focalHeuristic(
      const std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>>&
          solution) {
    checkCanceled();
    int numConflicts = 0;

    int max_t = 0;
    for (const auto& sol : solution) {
      max_t = std::max<int>(max_t, sol.states.size() - 1);
    }

    for (int t = 0; t < max_t; ++t) {
      // check drive-drive vertex collisions
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1 = getState(i, solution, t);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2 = getState(j, solution, t);
          if (state1.equalExceptTime(state2)) {
            ++numConflicts;
          }
        }
      }
      // drive-drive edge (swap)
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1a = getState(i, solution, t);
        State state1b = getState(i, solution, t + 1);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2a = getState(j, solution, t);
          State state2b = getState(j, solution, t + 1);
          if (state1a.equalExceptTime(state2b) &&
              state1b.equalExceptTime(state2a)) {
            ++numConflicts;
          }
        }
      }
    }
    return numConflicts;
  }

  bool isSolution(const State& s) {
    checkCanceled();
    if (m_agentIdx >= m_goals.size()) return false;
    return s.vertex == m_goals[m_agentIdx] && s.time > m_lastGoalConstraint;
  }

  void getNeighbors(
      const State& s,
      std::vector<libMultiRobotPlanning::Neighbor<State, Action, int>>&
          neighbors) {
    checkCanceled();
    neighbors.clear();
    // Make moving strictly more expensive than waiting to reduce back-and-forth detours.
    static constexpr int kMinMoveCost = 2;
    static constexpr int kMoveStepPenalty = 1;
    auto es = boost::out_edges(s.vertex, m_roadmap);
    for (auto eit = es.first; eit != es.second; ++eit) {
      vertex_t v = boost::target(*eit, m_roadmap);
      State n(s.time + 1, v);
      if (stateValid(n) && transitionValid(s.time, *eit)) {
        int moveCost = m_roadmap[*eit].cost;
        if (isApVertex(v)) {
          moveCost += kAvoidWaitTraversalPenalty;
        }
        moveCost = std::max(moveCost + kMoveStepPenalty, kMinMoveCost);
        neighbors.emplace_back(
            libMultiRobotPlanning::Neighbor<State, Action, int>(
                n, *eit, moveCost));
      }
    }

    // Wait action
    if (!isApVertex(s.vertex)) {
      State n(s.time + 1, s.vertex);
      if (stateValid(n)) {
        neighbors.emplace_back(
            libMultiRobotPlanning::Neighbor<State, Action, int>(n, edge_t(),
                                                                1));
      }
    }
  }

  bool getFirstConflict(
      const std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>>&
          solution,
      Conflict& result) {
    checkCanceled();
    size_t max_t = 0;
    for (const auto& sol : solution) {
      max_t = std::max<size_t>(max_t, sol.states.size() - 1);
    }

    for (size_t t = 0; t <= max_t; ++t) {
      // check vertex/vertex collisions
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1 = getState(i, solution, t);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2 = getState(j, solution, t);
          if (state1.equalExceptTime(state2)) {
            result.time = t;
            result.agent1 = i;
            result.agent2 = j;
            result.type = Conflict::Vertex;
            result.vertex = state1.vertex;
            return true;
          }
          const auto& ba1 = blockAreaForVertex(state1.vertex);
          const auto& ba2 = blockAreaForVertex(state2.vertex);
          if (!ba1.empty() && ba1 == ba2) {
            result.time = t;
            result.agent1 = i;
            result.agent2 = j;
            result.type = Conflict::BlockArea;
            result.blockAreaId = ba1;
            return true;
          }
        }
      }
      // edge/edge (swap) - check conflictingEdges
      for (size_t i = 0; i < solution.size(); ++i) {
        for (size_t j = i + 1; j < solution.size(); ++j) {
          if (t < solution[i].actions.size() &&
              t < solution[j].actions.size()) {
            auto e1 = solution[i].actions[t].first;
            auto e2 = solution[j].actions[t].first;
            if (e1.m_eproperty) {
              const auto& ce = m_roadmap[e1].conflictingEdges;
              if (ce.find(e2) != ce.end()) {
                result.time = t;
                result.agent1 = i;
                result.agent2 = j;
                result.type = Conflict::Edge;
                result.edge1 = e1;
                result.edge2 = e2;
                return true;
              }
            }
          }
        }
      }
    }

    return false;
  }

  void createConstraintsFromConflict(
      const Conflict& conflict, std::map<size_t, Constraints>& constraints) {
    if (conflict.type == Conflict::Vertex) {
      Constraints c1;
      c1.vertexConstraints.emplace(
          VertexConstraint(conflict.time, conflict.vertex));
      constraints[conflict.agent1] = c1;
      constraints[conflict.agent2] = c1;
    } else if (conflict.type == Conflict::Edge) {
      Constraints c1;
      c1.edgeConstraints.emplace(EdgeConstraint(conflict.time, conflict.edge1));
      constraints[conflict.agent1] = c1;
      Constraints c2;
      c2.edgeConstraints.emplace(EdgeConstraint(conflict.time, conflict.edge2));
      constraints[conflict.agent2] = c2;
    } else if (conflict.type == Conflict::BlockArea) {
      const auto& vertices = blockAreaVertices(conflict.blockAreaId);
      if (!vertices.empty()) {
        Constraints c1;
        Constraints c2;
        for (auto v : vertices) {
          c1.vertexConstraints.emplace(VertexConstraint(conflict.time, v));
          c2.vertexConstraints.emplace(VertexConstraint(conflict.time, v));
        }
        constraints[conflict.agent1] = c1;
        constraints[conflict.agent2] = c2;
      }
    }
  }

  void onExpandHighLevelNode(int /*cost*/) { m_highLevelExpanded++; }

  void onExpandLowLevelNode(const State& /*s*/, int /*fScore*/,
                            int /*gScore*/) {
    m_lowLevelExpanded++;
  }

  int highLevelExpanded() { return m_highLevelExpanded; }

  int lowLevelExpanded() const { return m_lowLevelExpanded; }

 private:
  static constexpr int kAvoidWaitTraversalPenalty =
      10;  // 数字不能太大不然算不出来 保持现状
  using BlockAreaVertices = std::unordered_map<std::string, std::vector<vertex_t>>;

  void checkCanceled() const {
    if (m_cancelCallback && m_cancelCallback()) {
      throw CanceledSearch();
    }
  }

  State getState(
      size_t agentIdx,
      const std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>>&
          solution,
      size_t t) {
    checkCanceled();
    assert(agentIdx < solution.size());
    if (t < solution[agentIdx].states.size()) {
      return solution[agentIdx].states[t].first;
    }
    assert(!solution[agentIdx].states.empty());
    if (m_disappearAtGoal) {
      return State(-1, -1 - agentIdx);
    }
    return solution[agentIdx].states.back().first;
  }

  bool stateValid(const State& s) {
    checkCanceled();
    assert(m_constraints);
    const auto& con = m_constraints->vertexConstraints;
    return con.find(VertexConstraint(s.time, s.vertex)) == con.end();
  }

  bool transitionValid(int time, edge_t edge) {
    checkCanceled();
    assert(m_constraints);
    const auto& con = m_constraints->edgeConstraints;
    return con.find(EdgeConstraint(time, edge)) == con.end();
  }

  bool isApVertex(vertex_t vertex) const {
    if (vertex >= boost::num_vertices(m_roadmap)) {
      return false;
    }
    return m_roadmap[vertex].avoidWait;
  }

  const std::string& blockAreaForVertex(vertex_t v) const {
    static const std::string kEmpty;
    if (v >= m_vertexBlockArea.size()) {
      return kEmpty;
    }
    return m_vertexBlockArea[v];
  }

  const std::vector<vertex_t>& blockAreaVertices(const std::string& areaId) const {
    static const std::vector<vertex_t> kEmpty;
    auto it = m_blockAreaVertices.find(areaId);
    return it != m_blockAreaVertices.end() ? it->second : kEmpty;
  }

 private:
  const roadmap_t& m_roadmap;
  std::vector<vertex_t> m_goals;
  size_t m_agentIdx;
  const Constraints* m_constraints;
  int m_lastGoalConstraint;
  int m_highLevelExpanded;
  int m_lowLevelExpanded;
  bool m_disappearAtGoal;
  std::function<bool()> m_cancelCallback;
  std::vector<std::string> m_vertexBlockArea;
  BlockAreaVertices m_blockAreaVertices;
};

struct MultiAgentSolver::Impl {
  roadmap_t roadmap;
  std::unordered_map<std::string, vertex_t> vertexMap;
};

namespace {
QString prefixed(const QString& message) {
  return QStringLiteral("[MultiAgentSolver] %1").arg(message);
}

QString vertexName(const roadmap_t& roadmap, vertex_t vertex) {
  if (vertex >= boost::num_vertices(roadmap)) {
    return QStringLiteral("#%1").arg(static_cast<int>(vertex));
  }
  const auto& descriptor = roadmap[vertex].name;
  if (descriptor.empty()) {
    return QStringLiteral("#%1").arg(static_cast<int>(vertex));
  }
  return QString::fromStdString(descriptor);
}

QString requestLabel(const AgentRequest& request) {
  return QStringLiteral("AGV %1 (%2 -> %3)")
      .arg(request.agvNumber)
      .arg(request.startId)
      .arg(request.goalId);
}
}  // namespace

}  // namespace pathfinding
}  // namespace application
namespace application {
namespace pathfinding {

void MultiAgentSolver::setLogger(std::shared_ptr<kernel::ILogger> logger) {
  logger_ = std::move(logger);
}

void MultiAgentSolver::setVerboseLogging(bool enabled) {
  verboseLogging_ = enabled;
}

void MultiAgentSolver::buildFromGraph(PathfindingGraphPtr graph) {
  impl_ = std::make_shared<Impl>();
  if (!graph) {
    logWarning(QStringLiteral("buildFromGraph received null graph; cleared previous roadmap"));
    return;
  }

  const auto& nodes = graph->getNodes();
  const auto& edges = graph->getEdges();

  logDebug(QStringLiteral("Building graph: nodes=%1 edges=%2")
              .arg(nodes.size())
              .arg(edges.size()));

  if (verboseLogging_) {
    QStringList nodeDescriptions;
    nodeDescriptions.reserve(nodes.size());
    for (const auto& n : nodes) {
      nodeDescriptions.append(
          QStringLiteral("%1(x=%2,y=%3,dir=%4)")
              .arg(n.id)
              .arg(n.x, 0, 'f', 3)
              .arg(n.y, 0, 'f', 3)
              .arg(n.direction, 0, 'f', 2));
    }
    if (!nodeDescriptions.isEmpty()) {
      logVerbose(QStringLiteral("Node list: %1").arg(nodeDescriptions.join(", ")));
    }
  }

  for (const auto& n : nodes) {
    auto v = boost::add_vertex(impl_->roadmap);
    impl_->roadmap[v].name = n.id.toStdString();
    bool avoidWait = n.avoidWait;
    if (!avoidWait && n.id.startsWith(QStringLiteral("AP"), Qt::CaseInsensitive)) {
      // Keep legacy guard: AP* 作为兜底高惩罚
      avoidWait = true;
    }
    impl_->roadmap[v].avoidWait = avoidWait;
    impl_->roadmap[v].blockAreaId = n.blockAreaId.toStdString();
    impl_->vertexMap.emplace(impl_->roadmap[v].name, v);
  }

  QHash<QString, edge_t> edgeMap;
  int addedEdges = 0;
  int skippedEdges = 0;

  for (const auto& e : edges) {
    std::string fromStr = e.fromNodeId.toStdString();
    std::string toStr = e.toNodeId.toStdString();

    auto itFrom = impl_->vertexMap.find(fromStr);
    auto itTo = impl_->vertexMap.find(toStr);
    if (itFrom == impl_->vertexMap.end() || itTo == impl_->vertexMap.end()) {
      skippedEdges++;
      logWarning(QStringLiteral("Skip edge %1 -> %2: missing node (fromExists=%3, toExists=%4)")
                     .arg(e.fromNodeId)
                     .arg(e.toNodeId)
                     .arg(itFrom != impl_->vertexMap.end())
                     .arg(itTo != impl_->vertexMap.end()));
      continue;
    }

    vertex_t vFrom = itFrom->second;
    vertex_t vTo = itTo->second;
    int cost = static_cast<int>(std::round(e.weight <= 0.0 ? 1.0 : e.weight));
    if (cost <= 0) {
      logWarning(QStringLiteral("Edge %1 -> %2 has non-positive cost=%3, forcing to 1")
                     .arg(e.fromNodeId)
                     .arg(e.toNodeId)
                     .arg(cost));
      cost = 1;
    }

    auto edgePair = boost::add_edge(vFrom, vTo, impl_->roadmap);
    edge_t edge = edgePair.first;
    impl_->roadmap[edge].cost = cost;

    QString edgeKey = e.fromNodeId + "-" + e.toNodeId;
    edgeMap.insert(edgeKey, edge);
    addedEdges++;

    logVerbose(QStringLiteral("Edge %1 -> %2 (type=%3, cost=%4)")
                   .arg(e.fromNodeId)
                   .arg(e.toNodeId)
                   .arg(e.pathType)
                   .arg(cost));
  }

  logDebug(QStringLiteral("Added %1 edges, skipped %2").arg(addedEdges).arg(skippedEdges));

  int conflictingPairs = 0;
  for (const auto& e : edges) {
    QString forwardKey = e.fromNodeId + "-" + e.toNodeId;
    QString reverseKey = e.toNodeId + "-" + e.fromNodeId;

    if (edgeMap.contains(forwardKey) && edgeMap.contains(reverseKey)) {
      edge_t e1 = edgeMap.value(forwardKey);
      edge_t e2 = edgeMap.value(reverseKey);
      impl_->roadmap[e1].conflictingEdges.insert(e2);
      impl_->roadmap[e2].conflictingEdges.insert(e1);
      conflictingPairs++;
      logVerbose(QStringLiteral("Conflicting edges registered: %1 <-> %2").arg(forwardKey, reverseKey));
    } else if (verboseLogging_) {
      logVerbose(QStringLiteral("Reverse edge missing when setting conflicts for %1").arg(forwardKey));
    }
  }
  logDebug(QStringLiteral("Set conflicting edges for %1 edge pairs").arg(conflictingPairs));

  size_t actualEdgeCount = boost::num_edges(impl_->roadmap);
  logDebug(QStringLiteral("Verification: roadmap now has %1 edges").arg(actualEdgeCount));
  if (actualEdgeCount == 0) {
    logWarning(QStringLiteral("Roadmap contains zero edges after build"));
  }
}

bool MultiAgentSolver::solve(const std::vector<AgentRequest>& requests,
                             std::vector<AgentPlan>& plans,
                             double suboptimality) const {
  plans.clear();
  auto shouldStop = [this]() -> bool { return cancelCallback_ && cancelCallback_(); };
  if (shouldStop()) {
    logWarning(QStringLiteral("solve() aborted before start: canceled"));
    return false;
  }
  if (!impl_) {
    logWarning(QStringLiteral("solve() called before buildFromGraph; roadmap unavailable"));
    return false;
  }

  if (requests.empty()) {
    logWarning(QStringLiteral("solve() called with empty agent request list"));
    return false;
  }

  logDebug(QStringLiteral("Entry state: vertices=%1 edges=%2 vertexMap=%3 requests=%4")
              .arg(boost::num_vertices(impl_->roadmap))
              .arg(boost::num_edges(impl_->roadmap))
              .arg(impl_->vertexMap.size())
              .arg(requests.size()));

  for (const auto& request : requests) {
    if (shouldStop()) {
      logWarning(QStringLiteral("solve() aborted while resolving requests: canceled"));
      return false;
    }
    logDebug(QStringLiteral("  %1").arg(requestLabel(request)));
  }

  std::vector<State> startStates;
  std::vector<vertex_t> goalVertices;
  startStates.reserve(requests.size());
  goalVertices.reserve(requests.size());

  for (const auto& r : requests) {
    if (shouldStop()) {
      logWarning(QStringLiteral("solve() aborted while building start/goal: canceled"));
      return false;
    }
    auto itS = impl_->vertexMap.find(r.startId.toStdString());
    auto itG = impl_->vertexMap.find(r.goalId.toStdString());
    if (itS == impl_->vertexMap.end() || itG == impl_->vertexMap.end()) {
      logError(QStringLiteral("Graph lookup failed for %1 (startFound=%2, goalFound=%3)")
                   .arg(requestLabel(r))
                   .arg(itS != impl_->vertexMap.end())
                   .arg(itG != impl_->vertexMap.end()));
      return false;
    }
    startStates.emplace_back(State(0, itS->second));
    goalVertices.emplace_back(itG->second);
    logVerbose(QStringLiteral("Resolved vertices for %1: start=%2 goal=%3")
                   .arg(requestLabel(r))
                   .arg(vertexName(impl_->roadmap, itS->second))
                   .arg(vertexName(impl_->roadmap, itG->second)));
  }

  logDebug(QStringLiteral("Roadmap before Environment: vertices=%1 edges=%2 requests=%3")
              .arg(boost::num_vertices(impl_->roadmap))
              .arg(boost::num_edges(impl_->roadmap))
              .arg(requests.size()));

  bool disappearAtGoal = false;
  Environment env(impl_->roadmap, goalVertices, disappearAtGoal);
  env.setCancelCallback(cancelCallback_);

  std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>> solution;
  libMultiRobotPlanning::ECBS<State, Action, int, Conflict, Constraints, Environment> ecbs(env, suboptimality);

  logDebug(QStringLiteral("Running ECBS search (suboptimality=%1, disappearAtGoal=%2)")
              .arg(suboptimality, 0, 'f', 2)
              .arg(disappearAtGoal));
  bool success = false;
  try {
    success = ecbs.search(startStates, solution);
  } catch (const CanceledSearch&) {
    logWarning(QStringLiteral("ECBS search canceled by external request"));
    success = false;
  }

  logDebug(QStringLiteral("ECBS search result: %1 (solutionSize=%2 expected=%3)")
              .arg(success ? QStringLiteral("SUCCESS") : QStringLiteral("FAILED"))
              .arg(solution.size())
              .arg(requests.size()));

  if (!success) {
    logWarning(QStringLiteral("ECBS search failed - running BFS diagnostics"));
    for (size_t i = 0; i < requests.size(); ++i) {
      vertex_t start = startStates[i].vertex;
      vertex_t goal = goalVertices[i];
      logDebug(QStringLiteral("  Diagnostics for %1 (vertex %2 -> %3)")
                  .arg(requestLabel(requests[i]))
                  .arg(static_cast<int>(start))
                  .arg(static_cast<int>(goal)));

      std::unordered_set<vertex_t> visited;
      std::unordered_map<vertex_t, vertex_t> parents;
      std::queue<vertex_t> queue;
      queue.push(start);
      visited.insert(start);
      bool pathExists = false;

      while (!queue.empty() && !pathExists) {
        vertex_t current = queue.front();
        queue.pop();

        if (current == goal) {
          pathExists = true;
          break;
        }

        auto outEdges = boost::out_edges(current, impl_->roadmap);
        for (auto it = outEdges.first; it != outEdges.second; ++it) {
          vertex_t target = boost::target(*it, impl_->roadmap);
          if (visited.insert(target).second) {
            parents.emplace(target, current);
            queue.push(target);
          }
        }
      }

      logDebug(QStringLiteral("    Path exists (BFS): %1").arg(pathExists ? QStringLiteral("YES")
                                                                        : QStringLiteral("NO")));
      if (pathExists && verboseLogging_) {
        QStringList bfsPath;
        vertex_t cursor = goal;
        bfsPath.prepend(vertexName(impl_->roadmap, cursor));
        while (cursor != start) {
          auto itParent = parents.find(cursor);
          if (itParent == parents.end()) {
            break;
          }
          cursor = itParent->second;
          bfsPath.prepend(vertexName(impl_->roadmap, cursor));
        }
        logVerbose(QStringLiteral("    BFS path: %1").arg(bfsPath.join(" -> ")));
      }

      if (!pathExists) {
        logWarning(QStringLiteral("    Start vertex '%1' out-degree=%2")
                       .arg(vertexName(impl_->roadmap, start))
                       .arg(boost::out_degree(start, impl_->roadmap)));
        logWarning(QStringLiteral("    Goal vertex '%1'").arg(vertexName(impl_->roadmap, goal)));
        logError(QStringLiteral("    Graph is not connected between start and goal"));
      }
    }
    return false;
  }

  if (solution.size() != requests.size()) {
    logError(QStringLiteral("Solution size mismatch: solution=%1 expected=%2")
                 .arg(solution.size())
                 .arg(requests.size()));
    return false;
  }

  logDebug(QStringLiteral("===== MAPF Calculation Result ====="));
  for (size_t i = 0; i < solution.size(); ++i) {
    logDebug(QStringLiteral("AGV %1 route plan:").arg(requests[i].agvNumber));
    for (const auto& state : solution[i].states) {
      vertex_t v = state.first.vertex;
      int time = state.first.time;
      logDebug(QStringLiteral("  [t=%1] %2").arg(time).arg(vertexName(impl_->roadmap, v)));
    }
  }
  logDebug(QStringLiteral("===== End of MAPF Result ====="));

  plans.resize(requests.size());
  for (size_t i = 0; i < requests.size(); ++i) {
    plans[i].agvNumber = requests[i].agvNumber;
    for (const auto& state : solution[i].states) {
      vertex_t v = state.first.vertex;
      plans[i].route.append(QString::fromStdString(impl_->roadmap[v].name));
      plans[i].times.append(state.first.time);
    }
    if (plans[i].route.isEmpty() || plans[i].route.back() != requests[i].goalId) {
      plans[i].route.append(requests[i].goalId);
      plans[i].times.append(plans[i].times.isEmpty() ? 0 : plans[i].times.back() + 1);
    }
    if (verboseLogging_) {
      logVerbose(QStringLiteral("Plan for AGV %1: %2")
                     .arg(plans[i].agvNumber)
                     .arg(plans[i].route.join(QStringLiteral(" -> "))));
    }
  }

  logDebug(QStringLiteral("Generated %1 agent plans").arg(plans.size()));
  return true;
}

void MultiAgentSolver::logDebug(const QString& message) const {
  const QString formatted = prefixed(message);
  if (logger_) {
    logger_->debug(formatted);
  } else {
    std::cout << formatted.toStdString() << std::endl;
  }
}

void MultiAgentSolver::logInfo(const QString& message) const {
  const QString formatted = prefixed(message);
  if (logger_) {
    logger_->info(formatted);
  } else {
    std::cout << formatted.toStdString() << std::endl;
  }
}

void MultiAgentSolver::logWarning(const QString& message) const {
  const QString formatted = prefixed(message);
  if (logger_) {
    logger_->warning(formatted);
  } else {
    std::cout << formatted.toStdString() << std::endl;
  }
}

void MultiAgentSolver::logError(const QString& message) const {
  const QString formatted = prefixed(message);
  if (logger_) {
    logger_->error(formatted);
  } else {
    std::cerr << formatted.toStdString() << std::endl;
  }
}

void MultiAgentSolver::logVerbose(const QString& message) const {
  if (verboseLogging_) {
    logDebug(message);
  }
}

}  // namespace pathfinding
}  // namespace application
